#pragma once
namespace graph
{
   namespace algorithm
   {
      template<typename value_type>
      bool alg_data_container_t<value_type>::has_key(vertex_id_t v_id) const
      {
         typename data_t::const_iterator it1 = data_.find(v_id);
         return it1 != data_.end();
      }
      
      template<typename value_type>
      value_type& alg_data_container_t<value_type>::get_value(vertex_id_t v_id)
      {
         return data_[v_id];
      }
      
      template<typename value_type>
      value_type const & alg_data_container_t<value_type>::get_value(vertex_id_t v_id) const
      {
         return data_.find(v_id)->second;
      }
      
      template<typename value_type>
      void alg_data_container_t<value_type>::set_value(vertex_id_t v_id, value_type val)
      {
         data_[v_id] = val;
      }
      template<typename value_type>
      value_type& alg_data_container_t<value_type>::operator[](vertex_id_t v_id)
      {
         return data_[v_id];
      }
      template<typename value_type>
      value_type const & alg_data_container_t<value_type>::operator[](vertex_id_t v_id) const
      {
         return data_.find(v_id)->second;
      }
      template<typename value_type>
      typename alg_data_container_t<value_type>::data_t::iterator alg_data_container_t<value_type>::begin()
      {
         return data_.begin();
      }
      template<typename value_type>
      typename alg_data_container_t<value_type>::data_t::iterator alg_data_container_t<value_type>::end()
      {
         return data_.end();
      }
      
      inline size_t min_heap_t::parent(size_t idx)
      {
         return idx / 2;
      }
      inline size_t min_heap_t::left(size_t idx)
      {
         return idx * 2;
      }
      inline size_t min_heap_t::right(size_t idx)
      {
         return idx * 2 + 1;
      }  
      
      inline void min_heap_t::min_heapify(size_t idx)
      {
         size_t l = left(idx);
         size_t r = right(idx);
         size_t min;
         if (l <= heap_size_ && heap_[l] < heap_[idx])
            min = l;
         else 
            min = idx;
         if (r <= heap_size_ && heap_[r] < heap_[min])
            min = r;
         
         if (min != idx)
         {
            vertex_id_position_map_[heap_[min].second] = idx;
            vertex_id_position_map_[heap_[idx].second] = min;
            std::swap(heap_[idx], heap_[min]);
            min_heapify(min);
         }
      }
      inline void min_heap_t::build_min_heap()
      {
         for(size_t i = heap_size_ / 2; i > 0; --i)
            min_heapify(i);
      }
      inline vertex_id_t min_heap_t::minimum() const
      {
         return heap_[1].second;
      }
      inline vertex_id_t min_heap_t::extract_min()
      {
         #ifdef _DEBUG
         if (empty())
            throw graph_exception_t("extract_min - empty()");
         #endif
         vertex_id_t id = heap_[1].second;
         lat_lon_t min = heap_[1].first;
         heap_[1] = heap_[heap_size_];
         vertex_id_position_map_.erase(id);
         vertex_id_position_map_[heap_[1].second] = 1;
         --heap_size_;
         min_heapify(1);
         #ifdef _DEBUG
         if (min > heap_[1].first)
            throw graph_exception_t("extract_min - min > minimum()");
         #endif
         return id;
      }
      inline void min_heap_t::dec_key_by_id(lat_lon_t key, vertex_id_t id)
      {
         size_t idx = vertex_id_position_map_[id];
         #ifdef _DEBUG
         if (heap_[idx].second != id)
            throw graph_exception_t("heap_[idx].second != id");
         #endif
         dec_key_by_idx(key, idx);
      }
      inline void min_heap_t::dec_key_by_idx(lat_lon_t key, size_t idx)
      {
         #ifdef _DEBUG
         if (idx > heap_size_)
            throw graph_exception_t("idx > heap_size_");
         #endif
         heap_element_t& elem = heap_[idx];
         vertex_id_t id = elem.second;
         #ifdef _DEBUG
         if (key > elem.first)
            throw graph_exception_t("key > elem.first");
         #endif
         elem.first = key;
         while(idx > 1 && heap_[parent(idx)].first > key)            
         {
            vertex_id_position_map_[id] = parent(idx);
            vertex_id_position_map_[heap_[parent(idx)].second] = idx;
            std::swap(heap_[parent(idx)], heap_[idx]);
            idx = parent(idx);
         }
      }   
      inline void min_heap_t::insert(lat_lon_t key, vertex_id_t id)
      {
         ++heap_size_;
         heap_.resize(heap_size_ + 1);
         heap_[heap_size_] = heap_element_t(key, id);
         vertex_id_position_map_[id] = heap_size_;
         dec_key_by_idx(key, heap_size_);
      }
      inline min_heap_t::container_t::const_iterator min_heap_t::begin()
      {
         return heap_.begin();
      }
      inline min_heap_t::container_t::const_iterator min_heap_t::end()
      {
         return heap_.end();
      }
      inline bool min_heap_t::empty() const
      {
         return heap_size_ == 0;
      }
      inline bool min_heap_t::has_key(vertex_id_t id) const
      {
         return vertex_id_position_map_.find(id) != vertex_id_position_map_.end();
      }
      inline size_t min_heap_t::size() const
      {
         return heap_size_;
      }
   }
}   
